{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2340,
   "metadata": {},
   "outputs": [],
   "source": [
    "from plotly.offline import init_notebook_mode, iplot\n",
    "\n",
    "import pandas as pd\n",
    "import plotly.graph_objects as go\n",
    "import math\n",
    "import numpy as np\n",
    "np.set_printoptions(threshold=sys.maxsize)\n",
    "import random\n",
    "from decimal import Decimal"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2341,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "def to_lonlat(point_xy, origin):\n",
    "    r = 6371000\n",
    "\n",
    "    o_lat = math.radians(Decimal(origin[0]))\n",
    "    o_lon = math.radians(Decimal(origin[1]))\n",
    "    cos_phi = math.cos(o_lat)\n",
    "    lat = Decimal(point_xy[1])/Decimal(r) + Decimal(o_lat)\n",
    "    lon = Decimal(point_xy[0])/Decimal((r*cos_phi)) + Decimal(o_lon)\n",
    "    return {'lon': math.degrees(lon), 'lat': math.degrees(lat)}\n",
    "\n",
    "\n",
    "def seg_lonlat(seg):\n",
    "    return [to_lonlat(i,origin) for i in seg]\n",
    "\n",
    "def plot_segment(fig,seg):\n",
    "\n",
    "    df = pd.DataFrame(seg)\n",
    "    fig.add_trace(go.Scattermapbox(\n",
    "        mode = \"markers+lines\",\n",
    "        lon = df['lon'],\n",
    "        lat = df['lat'],\n",
    "        marker = {'size': 10}))\n",
    "\n",
    "def wrap(x, low, high):\n",
    "    if (low <= x and x < high):\n",
    "        return x\n",
    "\n",
    "    range = high - low\n",
    "    inv_range = 1.0/range\n",
    "    num_wraps = math.floor((x - low) * inv_range)\n",
    "    return x - (range * num_wraps)\n",
    "\n",
    "\n",
    "def waypoint_from_heading_and_distance(lat_traffic, lon_traffic, bearing, dist):\n",
    "    r = 6371000\n",
    "    bearing = wrap(bearing, 0, 2*math.pi)\n",
    "    radius_ratio = dist / r\n",
    "\n",
    "    lat_traffic_rad = math.radians(lat_traffic)\n",
    "    lon_traffic_rad = math.radians(lon_traffic)\n",
    "\n",
    "    lat_end = math.asin(math.sin(lat_traffic_rad) * math.cos(radius_ratio) +\n",
    "                        math.cos(lat_traffic_rad) * math.sin(radius_ratio) * math.cos(bearing))\n",
    "\n",
    "    lon_end = lon_traffic_rad + math.atan2(math.sin(bearing) * math.sin(radius_ratio) * math.cos(\n",
    "        lat_traffic_rad), math.cos(radius_ratio) - math.sin(lat_traffic_rad) * math.sin(lat_end))\n",
    "\n",
    "    lat_end = math.degrees(lat_end)\n",
    "    lon_end = math.degrees(lon_end)\n",
    "\n",
    "    return lat_end, lon_end, bearing, radius_ratio\n",
    "\n",
    "\n",
    "def get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end):\n",
    "\n",
    "    lat_start_rad = math.radians(lat_start)\n",
    "    lat_end_rad = math.radians(lat_end)\n",
    "\n",
    "    cos_lat_end = math.cos(lat_end_rad)\n",
    "    d_lon = math.radians(lon_end - lon_start)\n",
    "\n",
    "    y = (math.sin(d_lon) * cos_lat_end)\n",
    "    x = (math.cos(lat_start_rad) * math.sin(lat_end_rad) -\n",
    "         math.sin(lat_start_rad) * cos_lat_end * math.cos(d_lon))\n",
    "\n",
    "    return wrap(math.atan2(y, x), -math.pi, math.pi)\n",
    "\n",
    "\n",
    "def get_distance_to_next_waypoint(lat_uav, lon_uav, lat_end, lon_end):\n",
    "\n",
    "    r = 6371000\n",
    "\n",
    "    lat_uav_rad = math.radians(lat_uav)\n",
    "    lat_end_rad = math.radians(lat_end)\n",
    "\n",
    "    d_lat = lat_end_rad - lat_uav_rad\n",
    "    d_lon = math.radians(lon_end) - math.radians(lon_uav)\n",
    "\n",
    "    a = math.sin(d_lat / 2.0) * math.sin(d_lat / 2.0) + math.sin(d_lon / 2.0) * math.sin(d_lon / 2.0) * math.cos(lat_uav_rad) * math.cos(\n",
    "        lat_end_rad)\n",
    "\n",
    "    c = math.atan2(math.sqrt(a), math.sqrt(1.0 - a))\n",
    "\n",
    "    return r * 2.0 * c\n",
    "\n",
    "\n",
    "def get_distance_to_line(lat_uav, lon_uav, lat_traffic, lon_traffic, lat_end, lon_end):\n",
    "\n",
    "    return_value = 1\n",
    "    crosstrack_distance = 0.0\n",
    "\n",
    "    dist_to_end = get_distance_to_next_waypoint(\n",
    "        lat_uav, lon_uav, lat_end, lon_end)\n",
    "\n",
    "    if (dist_to_end < 0.1):\n",
    "        return_value = 0 \n",
    "\n",
    "    bearing_end = get_bearing_to_next_waypoint(\n",
    "        lat_uav, lon_uav, lat_end, lon_end)\n",
    "    bearing_track = get_bearing_to_next_waypoint(\n",
    "        lat_traffic, lon_traffic, lat_end, lon_end)\n",
    "    bearing_diff = wrap(bearing_track - bearing_end, -math.pi, math.pi)\n",
    "\n",
    "    if (bearing_diff > math.pi/2 or bearing_diff < -math.pi/2):\n",
    "        return_value = 0\n",
    "\n",
    "    crosstrack_distance = (dist_to_end) * math.sin(bearing_diff)\n",
    "\n",
    "    return return_value, crosstrack_distance \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2342,
   "metadata": {},
   "outputs": [],
   "source": [
    "#uav location \n",
    "origin=[32.617013,-96.490564]\n",
    "h = 0 #x\n",
    "k = 0 #y\n",
    "uav_z = 1000\n",
    "\n",
    "vx_now = 0\n",
    "vy_now = 0\n",
    "vz_now = 0\n",
    "\n",
    "crosstrack_separation = 500\n",
    "vertical_separation = 500\n",
    "collision_time_threshold = 60.0\n",
    "\n",
    "rot_step = 1.86\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2343,
   "metadata": {},
   "outputs": [],
   "source": [
    "lines=[\n",
    "    [[10000,10000],[h,k]],\n",
    "    [[1000,-1000],[h,k]],\n",
    "    [[-5000,-4000],[h,k]],\n",
    "    [[-3000,3000],[h,k]],\n",
    "    [[-200,200],[h,k]],\n",
    "    [[0,0],[h,k]],\n",
    "    [[50,100],[h,k]],\n",
    "    [[1000000,9000000],[h,k]],\n",
    "    [[800,30],[h,k]],\n",
    "    [[7000,3],[h,k]],\n",
    "]\n",
    "\n",
    "fig = go.Figure()\n",
    "    \n",
    "[plot_segment(fig,seg_lonlat(i)) for i in lines]\n",
    "\n",
    "fig.update_layout(\n",
    "    margin ={'l':0,'t':0,'b':0,'r':0},\n",
    "    mapbox = {\n",
    "        'center': {'lon': origin[1], 'lat': origin[0]},\n",
    "        'style': \"stamen-terrain\",\n",
    "        'zoom': 13})\n",
    "\n",
    "fig.write_html('traffic.html', auto_open=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2344,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "dist_xy = []\n",
    "\n",
    "for i in range(len(lines)):\n",
    "    r = math.sqrt(lines[i][0][0]*lines[i][0][0] + lines[i][0][1]*lines[i][0][1])\n",
    "    dist_xy.append(r)  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2345,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = [seg_lonlat(i) for i in lines]\n",
    "\n",
    "lat = []\n",
    "lon = []\n",
    "address = []\n",
    "\n",
    "for i in range(0,len(a)):\n",
    "    #print(str(a[i][0]['lat'])+\",\"+str(a[i][0]['lon']))\n",
    "    lat.append(a[i][0]['lat'])\n",
    "    lon.append(a[i][0]['lon'])\n",
    "    address.append(random.randint(1000,9999))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2364,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[6751, 2839, 2431, 3222, 2171, 7666, 4249, 8438, 4110, 5798]\n"
     ]
    }
   ],
   "source": [
    "print(address)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2346,
   "metadata": {},
   "outputs": [],
   "source": [
    "heading = np.arange(-math.pi, math.pi, math.radians(rot_step)) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2347,
   "metadata": {},
   "outputs": [],
   "source": [
    "headings = []\n",
    "\n",
    "ct_dist = []\n",
    "ct_distance_conflict_threshold = []\n",
    "\n",
    "vertical_arr = []\n",
    "vertical_check = []\n",
    "\n",
    "dist_xyz = []\n",
    "traffic_vel = []\n",
    "time_check = []\n",
    "collision_times = []"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2348,
   "metadata": {},
   "outputs": [],
   "source": [
    "for j in range(len(dist_xy)):\n",
    "\n",
    "    prediction_dist = dist_xy[j] + 1000     \n",
    "\n",
    "    for i in heading:\n",
    "\n",
    "        headings.append(i)       \n",
    "\n",
    "        lat_end, lon_end, bearing, radius_ratio = waypoint_from_heading_and_distance(lat[j],lon[j],i,prediction_dist)\n",
    "        \n",
    "\n",
    "        return_value, ct  = get_distance_to_line(origin[0],origin[1],lat[j],lon[j],lat_end, lon_end)\n",
    "\n",
    "        ct = abs(ct)\n",
    "\n",
    "        #print(lat_end,lon_end, bearing, radius_ratio, return_value, ct)\n",
    "\n",
    "        ct_dist.append(ct)\n",
    "        if(ct < crosstrack_separation and return_value):\n",
    "            ct_distance_conflict_threshold.append(True)\n",
    "        else:\n",
    "            ct_distance_conflict_threshold.append(False)\n",
    "\n",
    "        vertical_separation_check = random.choice([True, False])\n",
    "        if (vertical_separation_check):\n",
    "            z_diff = random.randrange(0, vertical_separation-1)\n",
    "            vertical_check.append(True)\n",
    "        else:\n",
    "            z_diff = random.randrange(vertical_separation+1, 1000)\n",
    "            vertical_check.append(False)\n",
    "            \n",
    "        sign = random.choice([-1, 1])\n",
    "        vertical_arr.append(sign*z_diff+uav_z)\n",
    "\n",
    "        xyz = math.sqrt(z_diff*z_diff + dist_xy[j]*dist_xy[j])\n",
    "        dist_xyz.append(xyz)\n",
    "\n",
    "        collision_time_check = random.choice([True, False])\n",
    "        if (collision_time_check):\n",
    "            time_to_collision = random.randrange(1, collision_time_threshold)\n",
    "            time_check.append(True)\n",
    "            collision_times.append(time_to_collision)\n",
    "        else:\n",
    "            time_to_collision = random.randrange(collision_time_threshold+1, 10*collision_time_threshold)\n",
    "            time_check.append(False)\n",
    "            collision_times.append(time_to_collision)\n",
    "\n",
    "        vel = xyz/time_to_collision\n",
    "        traffic_vel.append(math.sqrt((vel*vel)/2))\n",
    "\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2349,
   "metadata": {},
   "outputs": [],
   "source": [
    "lat_traffic = np.array([])\n",
    "\n",
    "for i in lat:\n",
    "    for j in range(len(heading)):\n",
    "        lat_now = np.array([i])\n",
    "        lat_traffic = np.concatenate([lat_traffic, lat_now])\n",
    "\n",
    "\n",
    "lon_traffic = np.array([])\n",
    "\n",
    "for i in lon:\n",
    "    for j in range(len(heading)):\n",
    "        lon_now = np.array([i])\n",
    "        lon_traffic = np.concatenate([lon_traffic, lon_now])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2350,
   "metadata": {},
   "outputs": [],
   "source": [
    "in_conflict = []\n",
    "\n",
    "for i in range(len(time_check)):\n",
    "    if((time_check[i]== True) and (vertical_check[i] == True) and (ct_distance_conflict_threshold[i] == True)):\n",
    "        in_conflict.append(True)\n",
    "    else:\n",
    "        in_conflict.append(False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2351,
   "metadata": {},
   "outputs": [],
   "source": [
    "#print(vertical_check)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2352,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "946\n"
     ]
    }
   ],
   "source": [
    "print(vertical_arr[78])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2353,
   "metadata": {},
   "outputs": [],
   "source": [
    "#print(headings)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2354,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "13903.21572374496\n"
     ]
    }
   ],
   "source": [
    "print(ct_dist[78])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2355,
   "metadata": {},
   "outputs": [],
   "source": [
    "#print(ct_distance_conflict_threshold)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2356,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "False\n"
     ]
    }
   ],
   "source": [
    "print(time_check[78])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2357,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "456"
      ]
     },
     "execution_count": 2357,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "collision_times[78]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2358,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "21.929984429241845"
      ]
     },
     "execution_count": 2358,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "traffic_vel[78]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2359,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "False\n"
     ]
    }
   ],
   "source": [
    "print(in_conflict[78])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2360,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "158\n"
     ]
    }
   ],
   "source": [
    "print(np.count_nonzero(in_conflict))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2361,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "dist_xy 10\n",
      "lat_traffic 1940\n",
      "lon_traffic 1940\n"
     ]
    }
   ],
   "source": [
    "print(\"dist_xy\", len(dist_xy))\n",
    "print(\"lat_traffic\", len(lat_traffic))\n",
    "print(\"lon_traffic\", len(lon_traffic))\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2362,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"      \t\n",
    "\tstruct traffic_data_s {\n",
    "\tdouble lat_traffic\n",
    "\tdouble lon_traffic\n",
    "\tfloat alt_traffic\n",
    "\tfloat heading_traffic\n",
    "\tfloat vxy_traffic\n",
    "\tfloat vz_traffic\n",
    "\tbool in_conflict\n",
    "\"\"\"\n",
    "\n",
    "traffic_data = np.zeros(shape=(7, len(lat_traffic)))\n",
    "\n",
    "traffic_data = np.concatenate([traffic_data,[lat_traffic]])\n",
    "\n",
    "traffic_data = np.concatenate([traffic_data,[lon_traffic]])\n",
    "\n",
    "\n",
    "alt_traffic = np.asarray(vertical_arr)\n",
    "traffic_data = np.concatenate([traffic_data,[alt_traffic]])\n",
    "\n",
    "headings_arr = np.asarray(headings)\n",
    "traffic_data = np.concatenate([traffic_data,[headings_arr]])\n",
    "\n",
    "vxy_traffic = np.asarray(traffic_vel)\n",
    "traffic_data = np.concatenate([traffic_data,[vxy_traffic]])\n",
    "\n",
    "vz_traffic = np.asarray(traffic_vel)\n",
    "traffic_data = np.concatenate([traffic_data,[vz_traffic]])\n",
    "\n",
    "in_conflict_arr = np.asarray(in_conflict)\n",
    "traffic_data = np.concatenate([traffic_data,[in_conflict_arr]])\n",
    "\n",
    "for i in range(7):\n",
    "\ttraffic_data = np.delete(traffic_data,0,0)\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2363,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(1940, 7)\n"
     ]
    }
   ],
   "source": [
    "np.savetxt(\"foo.csv\", traffic_data.transpose(), delimiter=\",\",newline='}, \\n {')\n",
    "print(traffic_data.transpose().shape)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  },
  "vscode": {
   "interpreter": {
    "hash": "e7370f93d1d0cde622a1f8e1c04877d8463912d04d973331ad4851f04de6915a"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
